#include <kf_pos_vel.hpp>

KfPosVel::KfPosVel()
{
	// 初始化状态向量 [位置, 速度]
	state << 0.0, 0.0;

	// 初始化误差协方差矩阵
	P << 1.0, 0.0,
		0.0, 1.0;

	// 初始化观测矩阵
	H << 1.0, 0.0,
		0.0, 1.0;
}

KfPosVel::~KfPosVel()
{
}

void KfPosVel::SetParam(double dt, double s, double r)
{
	// 状态转移矩阵
	F << 1.0, dt, // 位置
		0.0, 1.0; // 速度

	// 输入控制矩阵
	B << 0.5 * dt * dt, // 位置
		dt;				// 速度

	// 初始化过程噪声协方差矩阵 Q
	Q << s, 0.0,
		0.0, s;

	// 初始化观测噪声协方差矩阵
	R << r, 0,
		0, r / dt;
}

void KfPosVel::Update(double accel, double position, double velocity, double& est_pos, double& est_vel)
{
	// 将观测位置放入观测矩阵
	measurement << position, velocity;

	// 更新状态
	state = F * state + B * accel;
	// 更新过程误差协方差
	P = F * P * F.transpose() + Q;
	// 计算卡尔曼增益
	K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
	// 更新状态向量
	state = state + K * (measurement - H * state);
	// 更新协方差矩阵
	P = (Eigen::Matrix<double, 2, 2>::Identity() - K * H) * P;

	est_pos = state(0);
	est_vel = state(1);
}